#!/usr/bin/env python3
"""
Runway Touchdown Reward System
------------------------------
集成DCS飞行模拟器着陆系统，专注于跑道接地点奖励函数设计与评估。
该系统基于a416.py实现的下滑道控制和getDCSdata接口，实现飞机着陆过程中
接地点的精确定位和多维度评分。

主要功能：
1. 与DCS实时数据接口对接，获取飞机状态
2. 实现接地点检测和坐标记录
3. 基于接地位置、速度、姿态等参数计算综合奖励
4. 提供可视化分析和日志记录
"""

import math
import time
import os
import logging
import csv
import traceback
from datetime import datetime
from typing import Dict, List, Tuple, Optional
import numpy as np
import matplotlib

matplotlib.use('Agg')  # 设置非交互式后端
import matplotlib.pyplot as plt

# 确保结果目录存在
RESULTS_DIR = "results"
os.makedirs(RESULTS_DIR, exist_ok=True)

# 日志配置
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(levelname)s - %(message)s',
    handlers=[
        logging.FileHandler(os.path.join(RESULTS_DIR, "touchdown_system.log")),
        logging.StreamHandler()
    ]
)
logger = logging.getLogger(__name__)

# 跑道和着陆参数配置 - 与a416.py保持一致
# 跑道和着陆参数配置 - 与DescentEnv.py保持一致
RUNWAY_CONFIG = {
    'LATITUDE': 41.929631,  # 跑道入口纬度
    'LONGITUDE': 41.86325,  # 跑道入口经度
    'ALTITUDE': 18.0,  # 跑道海拔高度(米)
    'HEADING': 76.0,  # 跑道朝向(度)，从70.0更新为76.0
    'LENGTH': 2500,  # 跑道长度(米)
    'WIDTH': 45,  # 跑道宽度(米)
    'APPROACH_SPEED': 79.17,  # 理想进近速度(m/s)，从70.0更新为79.17
    'GLIDE_ANGLE': 3.0,  # 下滑道角度(度)
    'TOUCHDOWN_ZONE': {
        'START': 300,  # 理想着陆区起始点(距跑道头)
        'LENGTH': 300,  # 理想着陆区长度
    }
}


# 新增高度阈值常量
LOW_ALT_THRESHOLD = 350.0  # 起落架和襟翼触发高度
CRASH_ALT_THRESHOLD = RUNWAY_CONFIG['ALTITUDE'] - 50.0  # 坠毁高度阈值
SPEED_UPPER_LIMIT = 83.33  # 300 km/h ≈ 83.33 m/s
SPEED_LOWER_LIMIT = 55.56  # 200 km/h ≈ 55.56 m/s
# 接地奖励参数

TOUCHDOWN_REWARD_PARAMS = {
    'POSITION': {
        'IDEAL_WEIGHT': 0.4,  # 理想位置权重
        'MAX_REWARD': 100,  # 最大位置奖励
        'MIN_REWARD': -100  # 最小位置奖励(惩罚)
    },
    'SPEED': {
        'IDEAL': 65.28,  # 理想接地速度(m/s)，从70.0更新为65.28 (根据DescentEnv.py中的LANDING_SPEED)
        'TOLERANCE': 10.0,  # 速度容差(m/s)
        'WEIGHT': 0.2,  # 速度权重
        'MAX_REWARD': 50,  # 最大速度奖励
        'MIN_REWARD': -50  # 最小速度奖励(惩罚)
    },
    # ... 其他参数保持不变
    'VERTICAL_SPEED': {
        'IDEAL': -1.5,  # 调整理想垂直速度(m/s)，较原来更缓和
        'MAX': -3.0,  # 调整最大允许垂直速度(m/s)
        'WEIGHT': 0.2,
        'MAX_REWARD': 50,
        'MIN_REWARD': -100
    },
    'ATTITUDE': {
        'PITCH': {
            'IDEAL': 3.0,  # 理想俯仰角(度)
            'TOLERANCE': 2.0,  # 俯仰角容差(度)
            'WEIGHT': 0.1,  # 俯仰角权重
            'MAX_REWARD': 25,  # 最大俯仰角奖励
            'MIN_REWARD': -50  # 最小俯仰角奖励(惩罚)
        },
        'BANK': {
            'MAX': 5.0,  # 最大允许滚转角(度)
            'WEIGHT': 0.1,  # 滚转角权重
            'MAX_REWARD': 25,  # 最大滚转角奖励
            'MIN_REWARD': -50  # 最小滚转角奖励(惩罚)
        }
    }
}


class DCSDataHandler:
    """DCS数据处理类    负责与DCS模拟器通信获取飞行数据："""

    def __init__(self, max_retries: int = 3, retry_delay: float = 0.5):
        self.max_retries = max_retries
        self.retry_delay = retry_delay
        self.last_valid_data = None

    def get_dcs_data(self, verbose: bool = False) -> Optional[Dict]:
        """
        获取DCS数据，包含重试和错误处理

        Args:
            verbose: 是否输出详细日志

        Returns:
            DCS数据字典或None（如果无法获取数据）
        """
        # 导入getDCSdata模块
        try:
            from getDCSdata import get_dcs_data as dcs_get_data
        except ImportError as e:
            logger.error(f"无法导入DCS数据模块: {e}")
            return self.last_valid_data if self.last_valid_data else None

        # 尝试多次获取数据
        for attempt in range(self.max_retries):
            try:
                # 尝试从DCS获取数据
                data = dcs_get_data()

                if data is not None:
                    # 验证数据完整性
                    if self._validate_dcs_data(data):
                        if verbose:
                            logger.debug(
                                f"成功获取DCS数据: 高度={data.get('LLA_balt', 'N/A')}m, 速度={data.get('IAS', 'N/A')}m/s")
                        self.last_valid_data = data
                        return data
                    else:
                        logger.warning(f"DCS数据不完整 (尝试 {attempt + 1}/{self.max_retries})")
                else:
                    logger.warning(f"DCS返回空数据 (尝试 {attempt + 1}/{self.max_retries})")

                # 在重试前等待
                if attempt < self.max_retries - 1:
                    time.sleep(self.retry_delay)

            except Exception as e:
                logger.error(f"获取DCS数据时发生错误: {e}")
                if attempt < self.max_retries - 1:
                    time.sleep(self.retry_delay)

        # 如果所有尝试都失败，返回上次有效数据或模拟数据
        logger.warning("无法获取有效DCS数据，将使用上次有效数据或模拟数据")
        if self.last_valid_data:
            return self.last_valid_data
        else:
            return self._generate_simulated_data()

    def _validate_dcs_data(self, data: Dict) -> bool:
        """
        验证DCS数据的完整性

        Args:
            data: 要验证的DCS数据字典

        Returns:
            布尔值表示数据是否有效
        """
        required_fields = [
            "LLA_lat", "LLA_long", "LLA_balt",
            "IAS", "heading_deg", "pitch_deg", "bank_deg",
            "VV"
        ]

        for field in required_fields:
            if field not in data:
                logger.warning(f"DCS数据缺少必要字段: {field}")
                return False

        return True

    def _generate_simulated_data(self) -> Dict:
        """
        生成模拟的DCS数据用于测试

        Returns:
            模拟的DCS数据字典
        """
        logger.info("生成模拟DCS数据用于测试")

        runway_lat = RUNWAY_CONFIG['LATITUDE']
        runway_long = RUNWAY_CONFIG['LONGITUDE']
        runway_alt = RUNWAY_CONFIG['ALTITUDE']

        # 模拟一个在跑道附近下降的飞机
        return {
            "M_": 0.2,
            "IAS": 70.0,
            "heading_deg": RUNWAY_CONFIG['HEADING'],
            "pitch_deg": 3.0,
            "bank_deg": 0.5,
            "Point": [0, 100, 0],
            "VV": [70, 0, -3.0],  # x, y, z速度分量
            "VV_0": 70.1,  # 合速度
            "AoA": 4.0,
            "ACC": [0, 0, 0],
            "Fuel": 0.6,
            "Gears": 1.0,  # 起落架放下
            "Flaps": 0.75,  # 襟翼部分放下
            "AirBrake": 0.25,  # 减速板部分放下
            "RPM": [0.45, 0.45, 0, 0, 0, 0],  # 引擎转速
            "LLA_long": runway_long - 0.003,  # 跑道前方约300米
            "LLA_lat": runway_lat + 0.0005,
            "LLA_balt": runway_alt + 5.0,  # 跑道上方5米
            "Omega": [0.0, 0.0, 0.0]  # 角速度
        }


class TouchdownRewardSystem:
    """跑道接地着陆点奖励系统"""
    '''

    '''

    def __init__(self):
        # 初始化DCS数据处理器

        self.dcs_handler = DCSDataHandler()

        # 接地点检测状态
        self.touchdown_detected = False
        self.touchdown_position = None
        self.touchdown_time = None
        self.landing_complete = False

        # 接地历史记录
        self.touchdown_history = []

        # 控制参数
        self.last_control_time = time.time()
        self.control_interval = 0.2  # 控制周期（秒）

        # 创建数据文件
        self.data_file = os.path.join(RESULTS_DIR, f"touchdown_data_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv")
        self._init_data_file()

    def _init_data_file(self):
        """初始化CSV数据文件"""
        with open(self.data_file, 'w', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow([
                'timestamp', 'phase', 'altitude', 'speed', 'vertical_speed',
                'pitch', 'bank', 'heading', 'longitude', 'latitude',
                'distance_to_runway', 'glide_path_deviation',
                'touchdown_detected', 'touchdown_long', 'touchdown_lat',
                'position_reward', 'speed_reward', 'vs_reward',
                'pitch_reward', 'bank_reward', 'total_reward'
            ])

    def run(self, max_time: float = 600.0):
        """
        运行着陆奖励系统

        Args:
            max_time: 最大运行时间（秒）
        """
        try:
            # 激活DCS窗口
            # 激活DCS窗口
            try:
                from vjoy_control import activate_dcs_window
                activate_dcs_window()
                logger.info("DCS窗口已激活")
            except Exception as e:
                logger.warning(f"无法激活DCS窗口: {e}")

            start_time = time.time()
            last_status_time = start_time

            # 主循环
            while time.time() - start_time < max_time and not self.landing_complete:
                # 每0.5秒更新状态显示
                current_time = time.time()
                if current_time - last_status_time >= 0.5:
                    self._clear_screen()
                    data = self.dcs_handler.get_dcs_data()
                    self._display_status(data)
                    last_status_time = current_time

                # 获取飞行数据
                data = self.dcs_handler.get_dcs_data()
                if data is None:
                    logger.warning("未获取到DCS数据，重试中...")
                    time.sleep(0.5)
                    continue

                # 检测接地
                self._detect_touchdown(data)

                # 计算奖励
                rewards = self._calculate_rewards(data)

                # 记录数据
                self._record_data(data, rewards)

                # 检查是否完成
                if self._is_landing_complete(data):
                    self.landing_complete = True
                    self._generate_touchdown_analysis()
                    break

                # 控制循环频率
                time.sleep(0.1)

        except KeyboardInterrupt:
            logger.info("用户中断系统")
            self._generate_touchdown_analysis()
        except Exception as e:
            logger.error(f"系统运行错误: {str(e)}")
            logger.error(traceback.format_exc())
            self._generate_touchdown_analysis()

    def _detect_touchdown(self, data: Dict) -> bool:
        """
        检测接地事件 - 仅基于0-7米高度范围判断

        Args:
            data: DCS飞行数据

        Returns:
            是否检测到接地事件
        """
        if self.touchdown_detected:
            return True

        altitude = data["LLA_balt"]
        runway_alt = RUNWAY_CONFIG['ALTITUDE']

        # 修改接地条件：只要高度在跑道上方0-7米范围内即可，不再考虑垂直速度
        if 0 <= (altitude - runway_alt) <= 7.0:
            self.touchdown_detected = True
            self.touchdown_time = datetime.now()
            self.touchdown_position = (data["LLA_long"], data["LLA_lat"])

            logger.info(
                f"检测到接地事件! "
                f"位置=({data['LLA_long']:.6f}, {data['LLA_lat']:.6f}), "
                f"高度={altitude - runway_alt:.1f}m, "
                f"速度={data['IAS']:.1f}m/s, "
                f"垂直速度={data['VV'][2]:.1f}m/s"
            )

            return True

        return False

    def _is_landing_complete(self, data: Dict) -> bool:
        """
        判断着陆是否完成 - 适应新的高度范围

        Args:
            data: DCS飞行数据

        Returns:
            布尔值表示着陆是否完成
        """
        if not self.touchdown_detected:
            return False

        # 判断条件：接地后速度降至较低水平
        speed = data["IAS"]
        altitude = data["LLA_balt"]
        runway_alt = RUNWAY_CONFIG['ALTITUDE']

        # 修改着陆完成条件，允许0-7米高度范围
        return 0 <= (altitude - runway_alt) <= 7.0 and speed < 30.0

    def _calculate_rewards(self, data: Dict) -> Dict:
        """
        计算接地奖励

        Args:
            data: DCS飞行数据

        Returns:
            奖励字典
        """
        rewards = {
            'position_reward': 0,
            'speed_reward': 0,
            'vs_reward': 0,
            'pitch_reward': 0,
            'bank_reward': 0,
            'alt_threshold_reward': 0,  # 新增高度阈值奖励
            'total_reward': 0
        }

        # 提取关键参数
        altitude = data["LLA_balt"]
        speed = data["IAS"]
        vertical_speed = data["VV"][2]
        pitch = data["pitch_deg"]
        bank = data["bank_deg"]

        # 计算到跑道的距离
        distance_to_runway = self._calculate_distance_to_runway(data)

        # 在接地检测前，考虑高度阈值奖励
        if not self.touchdown_detected:
            # 检查飞机是否在适当的高度放下襟翼和起落架
            if altitude - RUNWAY_CONFIG['ALTITUDE'] < LOW_ALT_THRESHOLD and distance_to_runway < 3000:
                # 检查襟翼和起落架
                if data.get("Flaps", 0) > 0.5 and data.get("Gears", 0) > 0.5:
                    rewards['alt_threshold_reward'] = 25  # 正确配置奖励
                else:
                    rewards['alt_threshold_reward'] = -25  # 配置错误惩罚

            # 检查速度是否在适当范围内
            if speed < SPEED_LOWER_LIMIT:
                rewards['speed_reward'] = -25  # 速度过低惩罚
            elif speed > SPEED_UPPER_LIMIT:
                rewards['speed_reward'] = -50  # 速度过高严重惩罚

            # 检查坠毁风险
            if altitude < CRASH_ALT_THRESHOLD:
                rewards['alt_threshold_reward'] = -100  # 坠毁风险严重惩罚

            # 进近阶段总奖励计算
            if altitude - RUNWAY_CONFIG['ALTITUDE'] < LOW_ALT_THRESHOLD:
                # 最终进近阶段，更注重配置和稳定性
                target_speed = RUNWAY_CONFIG['APPROACH_SPEED']
                speed_error = abs(speed - target_speed)

                # 速度控制奖励
                if speed_error < 5:
                    rewards['speed_reward'] += 20
                elif speed_error < 10:
                    rewards['speed_reward'] += 10

                # 下滑道偏差奖励
                glide_path_deviation = self._calculate_glide_path_deviation(data)
                if abs(glide_path_deviation) < 10:
                    rewards['alt_threshold_reward'] += 20
                elif abs(glide_path_deviation) < 20:
                    rewards['alt_threshold_reward'] += 10

                # 接近奖励
                if distance_to_runway < 1000:
                    rewards['alt_threshold_reward'] += 10

                # 计算进近阶段总奖励
                rewards['total_reward'] = (
                        0.3 * rewards['speed_reward'] +
                        0.5 * rewards['alt_threshold_reward']
                )

            return rewards  # 未接地时返回部分奖励

        # 以下是已接地时的奖励计算

        # 1. 接地位置奖励
        position_reward = self._calculate_position_reward()
        rewards['position_reward'] = position_reward

        # 2. 速度奖励 - 更新为使用新的理想接地速度
        speed_reward = self._calculate_speed_reward(speed)
        rewards['speed_reward'] = speed_reward

        # 3. 垂直速度奖励
        vs_reward = self._calculate_vertical_speed_reward(vertical_speed)
        rewards['vs_reward'] = vs_reward

        # 4. 姿态奖励
        pitch_reward = self._calculate_pitch_reward(pitch)
        bank_reward = self._calculate_bank_reward(bank)
        rewards['pitch_reward'] = pitch_reward
        rewards['bank_reward'] = bank_reward

        # 5. 高级奖励 - 检查接地时的配置
        if data.get("Flaps", 0) < 0.5:
            rewards['alt_threshold_reward'] -= 20  # 没有放下襟翼惩罚
        if data.get("Gears", 0) < 0.5:
            rewards['alt_threshold_reward'] -= 50  # 没有放下起落架严重惩罚

        # 6. 速度范围检查
        if speed < SPEED_LOWER_LIMIT:
            rewards['speed_reward'] -= 25  # 接地速度过低惩罚
        elif speed > SPEED_UPPER_LIMIT:
            rewards['speed_reward'] -= 50  # 接地速度过高严重惩罚

        # 计算总奖励（带权重）通过加权组合各维度的奖励、实现了整体评价:
        total_reward = (
                TOUCHDOWN_REWARD_PARAMS['POSITION']['IDEAL_WEIGHT'] * position_reward +
                TOUCHDOWN_REWARD_PARAMS['SPEED']['WEIGHT'] * speed_reward +
                TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['WEIGHT'] * vs_reward +
                TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['WEIGHT'] * pitch_reward +
                TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['WEIGHT'] * bank_reward +
                0.1 * rewards['alt_threshold_reward']  # 添加高度阈值奖励的权重
        )

        rewards['total_reward'] = total_reward

        # 记录接地历史
        self.touchdown_history.append({
            'timestamp': datetime.now(),
            'position': self.touchdown_position,
            'speed': speed,
            'vertical_speed': vertical_speed,
            'pitch': pitch,
            'bank': bank,
            'rewards': rewards
        })

        if len(self.touchdown_history) % 5 == 0:  # 每5个数据点更新一次图表
            self._plot_episode_reward_over_time()
            logger.info(f"已更新奖励图表 (数据点: {len(self.touchdown_history)})")

        return rewards

    def _calculate_position_reward(self) -> float:
        """
        计算接地位置奖励

        Returns:
            位置奖励值
        """
        if not self.touchdown_position:
            return 0

        # 提取接地位置
        touchdown_long, touchdown_lat = self.touchdown_position

        # 计算接地点到跑道中心线的横向偏差和纵向位置
        runway_lat = RUNWAY_CONFIG['LATITUDE']
        runway_long = RUNWAY_CONFIG['LONGITUDE']
        runway_heading = math.radians(RUNWAY_CONFIG['HEADING'])
        runway_width = RUNWAY_CONFIG['WIDTH']
        ideal_touchdown_start = RUNWAY_CONFIG['TOUCHDOWN_ZONE']['START']
        ideal_touchdown_length = RUNWAY_CONFIG['TOUCHDOWN_ZONE']['LENGTH']

        # 将接地点转换为跑道坐标系
        dx, dy = self._transform_to_runway_coordinates(touchdown_long, touchdown_lat)

        # 横向偏差（到跑道中心线的距离）
        lateral_deviation = abs(dx)

        # 纵向位置（沿跑道方向的距离）
        longitudinal_position = dy

        # 计算横向奖励（基于与中心线的距离）
        max_lateral_reward = TOUCHDOWN_REWARD_PARAMS['POSITION']['MAX_REWARD'] / 2
        max_lateral_deviation = runway_width / 2

        if lateral_deviation <= max_lateral_deviation:
            # 在跑道内
            lateral_reward = max_lateral_reward * (1 - lateral_deviation / max_lateral_deviation)
        else:
            # 在跑道外
            lateral_reward = TOUCHDOWN_REWARD_PARAMS['POSITION']['MIN_REWARD'] / 2

        # 计算纵向奖励（基于着陆区）
        max_longitudinal_reward = TOUCHDOWN_REWARD_PARAMS['POSITION']['MAX_REWARD'] / 2

        # 理想着陆区域
        ideal_end = ideal_touchdown_start + ideal_touchdown_length

        if ideal_touchdown_start <= longitudinal_position <= ideal_end:
            # 完美着陆区域
            longitudinal_reward = max_longitudinal_reward
        elif 0 <= longitudinal_position < ideal_touchdown_start:
            # 着陆过早
            longitudinal_reward = max_longitudinal_reward * longitudinal_position / ideal_touchdown_start
        elif ideal_end < longitudinal_position <= RUNWAY_CONFIG['LENGTH']:
            # 着陆过晚
            remaining = RUNWAY_CONFIG['LENGTH'] - longitudinal_position
            total_remaining = RUNWAY_CONFIG['LENGTH'] - ideal_end
            longitudinal_reward = max_longitudinal_reward * (remaining / total_remaining)
        else:
            # 在跑道外着陆
            longitudinal_reward = TOUCHDOWN_REWARD_PARAMS['POSITION']['MIN_REWARD'] / 2

        # 总位置奖励
        return lateral_reward + longitudinal_reward

    def _transform_to_runway_coordinates(self, long: float, lat: float) -> Tuple[float, float]:
        """
        将地理坐标转换为相对于跑道的坐标

        Args:
            long: 经度
            lat: 纬度

        Returns:
            (x, y)元组，其中x为横向距离（米），y为纵向距离（米）
        """
        # 提取跑道参数
        runway_lat = RUNWAY_CONFIG['LATITUDE']
        runway_long = RUNWAY_CONFIG['LONGITUDE']
        runway_heading = math.radians(RUNWAY_CONFIG['HEADING'])

        # 计算地理差异（简化为平面近似）
        # 经度差与纬度差的近似米数转换
        dx = (long - runway_long) * 111320 * math.cos(math.radians(runway_lat))
        dy = (lat - runway_lat) * 110540

        # 计算跑道方向的单位向量
        runway_dx = math.sin(runway_heading)
        runway_dy = math.cos(runway_heading)

        # 将坐标旋转至跑道坐标系
        x = dx * runway_dy - dy * runway_dx  # 横向偏差（到跑道中心线的距离）
        y = dx * runway_dx + dy * runway_dy  # 纵向位置（沿跑道方向的距离）

        return (x, y)

    def _calculate_speed_reward(self, speed: float) -> float:
        """
        计算接地速度奖励

        Args:
            speed: 接地速度(m/s)

        Returns:
            速度奖励值
        """
        ideal_speed = TOUCHDOWN_REWARD_PARAMS['SPEED']['IDEAL']
        tolerance = TOUCHDOWN_REWARD_PARAMS['SPEED']['TOLERANCE']
        max_reward = TOUCHDOWN_REWARD_PARAMS['SPEED']['MAX_REWARD']
        min_reward = TOUCHDOWN_REWARD_PARAMS['SPEED']['MIN_REWARD']

        speed_error = abs(speed - ideal_speed)

        if speed <= ideal_speed + tolerance:
            # 在允许范围内
            return max_reward * (1 - speed_error / tolerance)
        elif speed <= ideal_speed + 2 * tolerance:
            # 速度过高但可接受
            excess = speed_error - tolerance
            return min_reward * (excess / tolerance)
        else:
            # 速度过高，危险
            return min_reward

    def _calculate_vertical_speed_reward(self, vertical_speed: float) -> float:
        """
        计算接地垂直速度奖励

        Args:
            vertical_speed: 垂直速度(m/s)

        Returns:
            垂直速度奖励值
        """
        ideal_vs = TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['IDEAL']
        max_vs = TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['MAX']
        max_reward = TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['MAX_REWARD']
        min_reward = TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['MIN_REWARD']

        if vertical_speed >= 0:
            # 上升状态，错误的接地
            return min_reward
        elif vertical_speed >= ideal_vs:
            # 下降率不足，但可接受
            return max_reward * (vertical_speed / ideal_vs)
        elif vertical_speed >= max_vs:
            # 理想下降率范围
            vs_ratio = (vertical_speed - max_vs) / (ideal_vs - max_vs)
            return max_reward * vs_ratio
        else:
            # 下降率过大，危险着陆
            return min_reward * (vertical_speed / max_vs)

    def _calculate_pitch_reward(self, pitch: float) -> float:
        """
        计算接地俯仰角奖励

        Args:
            pitch: 俯仰角(度)

        Returns:
            俯仰角奖励值
        """
        ideal_pitch = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['IDEAL']
        tolerance = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['TOLERANCE']
        max_reward = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['MAX_REWARD']
        min_reward = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['MIN_REWARD']

        pitch_error = abs(pitch - ideal_pitch)

        if pitch_error <= tolerance:
            # 理想俯仰角范围
            return max_reward * (1 - pitch_error / tolerance)
        elif pitch < 0:
            # 机头下压，非常危险
            return min_reward
        else:
            # 俯仰角偏离理想值
            return -pitch_error * 2

    def _calculate_bank_reward(self, bank: float) -> float:
        """
        计算接地滚转角奖励

        Args:
            bank: 滚转角(度)

        Returns:
            滚转角奖励值
        """
        max_bank = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MAX']
        max_reward = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MAX_REWARD']
        min_reward = TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MIN_REWARD']

        bank_abs = abs(bank)

        if bank_abs <= max_bank / 2:
            # 理想滚转角范围
            return max_reward * (1 - bank_abs / (max_bank / 2))
        elif bank_abs <= max_bank:
            # 可接受但不理想
            return max_reward * 0.5 * (1 - (bank_abs - max_bank / 2) / (max_bank / 2))
        else:
            # 滚转角过大，危险着陆
            return min_reward

    def _record_data(self, data: Dict, rewards: Dict):
        """
        记录飞行数据

        Args:
            data: DCS飞行数据
            rewards: 奖励数据
        """
        # 确定当前阶段
        altitude = data["LLA_balt"]
        distance_to_runway = self._calculate_distance_to_runway(data)

        if self.touchdown_detected:
            phase = "touchdown"
        elif altitude < 100 and distance_to_runway < 3000:
            phase = "landing"
        elif altitude < 1000 and distance_to_runway < 10000:
            phase = "approach"
        else:
            phase = "cruise"

        # 计算下滑道偏差
        glide_path_deviation = self._calculate_glide_path_deviation(data)

        # 提取接地位置
        touchdown_long = None
        touchdown_lat = None
        if self.touchdown_position:
            touchdown_long, touchdown_lat = self.touchdown_position

        # 写入CSV
        with open(self.data_file, 'a', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow([
                datetime.now().strftime('%H:%M:%S.%f'),
                phase,
                data["LLA_balt"],
                data["IAS"],
                data["VV"][2],  # 垂直速度
                data["pitch_deg"],
                data["bank_deg"],
                data["heading_deg"],
                data["LLA_long"],
                data["LLA_lat"],
                distance_to_runway,
                glide_path_deviation,
                self.touchdown_detected,
                touchdown_long,
                touchdown_lat,
                rewards['position_reward'],
                rewards['speed_reward'],
                rewards['vs_reward'],
                rewards['pitch_reward'],
                rewards['bank_reward'],
                rewards['total_reward']
            ])

    def _calculate_distance_to_runway(self, data: Dict) -> float:
        """
        计算到跑道入口的距离

        Args:
            data: DCS飞行数据

        Returns:
            到跑道入口的距离（米）
        """
        current_lat = data["LLA_lat"]
        current_long = data["LLA_long"]
        runway_lat = RUNWAY_CONFIG['LATITUDE']
        runway_long = RUNWAY_CONFIG['LONGITUDE']

        return self._calculate_geo_distance(current_lat, current_long, runway_lat, runway_long)

    def _calculate_geo_distance(self, lat1: float, lon1: float, lat2: float, lon2: float) -> float:
        """
        计算两点间的地理距离（米）

        Args:
            lat1: 第一点纬度
            lon1: 第一点经度
            lat2: 第二点纬度
            lon2: 第二点经度

        Returns:
            距离（米）
        """
        # 将经纬度转换为弧度
        lat1_rad = math.radians(lat1)
        lon1_rad = math.radians(lon1)
        lat2_rad = math.radians(lat2)
        lon2_rad = math.radians(lon2)

        # 地球半径（米）
        earth_radius = 6371000

        # 使用Haversine公式计算距离
        dlon = lon2_rad - lon1_rad
        dlat = lat2_rad - lat1_rad
        a = math.sin(dlat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2) ** 2
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
        distance = earth_radius * c

        return distance

    def _calculate_glide_path_deviation(self, data: Dict) -> float:
        """
        计算到理想下滑道的垂直偏差 - 适应新的高度范围

        Args:
            data: DCS飞行数据

        Returns:
            下滑道偏差（米，正值表示高于下滑道，负值表示低于下滑道）
        """
        # 提取必要参数
        current_alt = data["LLA_balt"]

        # 目标参数
        runway_alt = RUNWAY_CONFIG['ALTITUDE']
        glide_angle = math.radians(RUNWAY_CONFIG['GLIDE_ANGLE'])

        # 计算到跑道的距离
        distance_to_runway = self._calculate_distance_to_runway(data)

        # 计算理想高度 - 考虑接地区域高度范围
        ideal_landing_height = 3.5  # 0-7米的中间值
        # 在接近跑道时(小于1000米)，理想高度调整为适应新的接地高度范围
        if distance_to_runway < 1000:
            approach_ratio = distance_to_runway / 1000
            ideal_height = runway_alt + (distance_to_runway * math.tan(glide_angle) * approach_ratio +
                                         ideal_landing_height * (1 - approach_ratio))
        else:
            ideal_height = runway_alt + distance_to_runway * math.tan(glide_angle)

        # 计算高度偏差
        return current_alt - ideal_height

    def _is_landing_complete(self, data: Dict) -> bool:
        """
        判断着陆是否完成

        Args:
            data: DCS飞行数据

        Returns:
            布尔值表示着陆是否完成
        """
        if not self.touchdown_detected:
            return False

        # 判断条件：接地后速度降至较低水平
        speed = data["IAS"]
        altitude = data["LLA_balt"]
        runway_alt = RUNWAY_CONFIG['ALTITUDE']

        # 接地后且速度低于30m/s，视为着陆完成
        return abs(altitude - runway_alt) < 2.0 and speed < 30.0

    def _clear_screen(self):
        """清屏"""
        os.system('cls' if os.name == 'nt' else 'clear')

    def _display_status(self, data: Dict):
        """
        显示当前状态

        Args:
            data: DCS飞行数据
        """
        if data is None:
            print("\n无法获取DCS数据")
            return

        # 提取必要数据
        altitude = data["LLA_balt"]
        speed = data["IAS"]
        vertical_speed = data["VV"][2]
        pitch = data["pitch_deg"]
        bank = data["bank_deg"]
        heading = data["heading_deg"]

        # 计算到跑道距离
        distance_to_runway = self._calculate_distance_to_runway(data)

        # 计算下滑道偏差
        glide_path_deviation = self._calculate_glide_path_deviation(data)

        # 确定飞行阶段
        if self.touchdown_detected:
            phase = "着陆点评估"
            phase_color = "\033[92m"  # 绿色
        # 调整条件：只要高度在0-7米范围内就视为着陆阶段，不考虑距离
        elif 0 <= (altitude - RUNWAY_CONFIG['ALTITUDE']) <= 7.0:
            phase = "着陆阶段"
            phase_color = "\033[93m"  # 黄色
        elif altitude < 1000 and distance_to_runway < 10000:
            phase = "进近阶段"
            phase_color = "\033[94m"  # 蓝色
        else:
            phase = "巡航阶段"
            phase_color = "\033[0m"  # 正常

        # 获取奖励（如果已接地）
        rewards = {}
        if self.touchdown_detected:
            rewards = self._calculate_rewards(data)

        # 格式化输出
        print("\n===== 跑道接地点奖励系统 =====")
        print(f"当前阶段: {phase_color}{phase}\033[0m")
        print(f"系统状态: {'已接地' if self.touchdown_detected else '未接地'}")

        print("\n飞行数据:")
        print(f"高度: {altitude:.1f}m | 速度: {speed:.1f}m/s | 垂直速度: {vertical_speed:.1f}m/s")
        print(f"姿态: 俯仰={pitch:.1f}° | 滚转={bank:.1f}° | 航向={heading:.1f}°")
        print(f"跑道: 距离={distance_to_runway:.1f}m | 下滑道偏差={glide_path_deviation:.1f}m")

        if self.touchdown_detected and self.touchdown_position:
            td_long, td_lat = self.touchdown_position
            runway_x, runway_y = self._transform_to_runway_coordinates(td_long, td_lat)
            print(f"\n接地数据:")
            print(f"接地点: 经度={td_long:.6f}, 纬度={td_lat:.6f}")
            print(f"跑道相对位置: 横向={runway_x:.1f}m, 纵向={runway_y:.1f}m")
            print(f"接地速度: {speed:.1f}m/s | 垂直速度: {vertical_speed:.1f}m/s")

            print("\n奖励评估:")
            print(f"位置奖励: {rewards.get('position_reward', 0):.1f}")
            print(f"速度奖励: {rewards.get('speed_reward', 0):.1f}")
            print(f"垂直速度奖励: {rewards.get('vs_reward', 0):.1f}")
            print(f"姿态奖励: 俯仰={rewards.get('pitch_reward', 0):.1f}, 滚转={rewards.get('bank_reward', 0):.1f}")
            print(f"总奖励: {rewards.get('total_reward', 0):.1f}")

        # 控制提示
        print("\n按Ctrl+C退出系统")

    def _generate_touchdown_analysis(self):
        """生成接地分析结果"""
        if not self.touchdown_detected or not self.touchdown_history:
            logger.info("未检测到接地事件，无法生成分析")
            return

        try:
            # 只计算着陆质量等级
            quality = self._determine_landing_quality()

            # 只生成奖励图
            self._plot_episode_reward_over_time()

            logger.info(f"接地分析完成，着陆质量：{quality}")

        except Exception as e:
            logger.error(f"生成接地分析时出错: {str(e)}")
            logger.error(traceback.format_exc())

    def _determine_landing_quality(self) -> str:
        """
        根据接地奖励确定着陆质量

        Returns:
            着陆质量等级
        """
        if not self.touchdown_history:
            return "无法评估"

        # 使用最后一条记录的总奖励
        final_reward = self.touchdown_history[-1]['rewards']['total_reward']

        if final_reward >= 90:
            return "完美 (Perfect)"
        elif final_reward >= 75:
            return "优秀 (Excellent)"
        elif final_reward >= 60:
            return "良好 (Good)"
        elif final_reward >= 40:
            return "合格 (Acceptable)"
        elif final_reward >= 20:
            return "勉强 (Marginal)"
        else:
            return "危险 (Dangerous)"

    def _generate_assessment_report(self, quality: str):
        """
        生成着陆评估报告

        Args:
            quality: 着陆质量等级
        """
        if not self.touchdown_history:
            return

        try:
            # 获取最后一条历史记录
            final_record = self.touchdown_history[-1]
            position = final_record['position']
            speed = final_record['speed']
            vertical_speed = final_record['vertical_speed']
            pitch = final_record['pitch']
            bank = final_record['bank']
            rewards = final_record['rewards']

            # 计算跑道相对位置
            runway_x, runway_y = self._transform_to_runway_coordinates(position[0], position[1])

            # 创建报告文件
            report_file = os.path.join(RESULTS_DIR, f"touchdown_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.txt")

            with open(report_file, 'w') as f:
                f.write("=== 跑道接地点评估报告 ===\n\n")
                f.write(f"着陆时间: {self.touchdown_time.strftime('%Y-%m-%d %H:%M:%S')}\n")
                f.write(f"着陆质量: {quality}\n\n")

                f.write("接地位置分析:\n")
                f.write(f"  经度: {position[0]:.6f}\n")
                f.write(f"  纬度: {position[1]:.6f}\n")
                f.write(f"  跑道相对位置:\n")
                f.write(
                    f"    横向偏差: {runway_x:.1f}m {'(跑道外)' if abs(runway_x) > RUNWAY_CONFIG['WIDTH'] / 2 else ''}\n")
                f.write(f"    纵向位置: {runway_y:.1f}m\n")

                ideal_start = RUNWAY_CONFIG['TOUCHDOWN_ZONE']['START']
                ideal_end = ideal_start + RUNWAY_CONFIG['TOUCHDOWN_ZONE']['LENGTH']

                if 0 <= runway_y < ideal_start:
                    location_note = "着陆过早：在理想着陆区之前"
                elif ideal_start <= runway_y <= ideal_end:
                    location_note = "理想着陆位置"
                elif ideal_end < runway_y <= RUNWAY_CONFIG['LENGTH']:
                    location_note = "着陆过晚：在理想着陆区之后"
                else:
                    location_note = "跑道外着陆"

                f.write(f"    位置评估: {location_note}\n")

                f.write("\n接地参数分析:\n")
                f.write(f"  速度: {speed:.1f}m/s (目标: {TOUCHDOWN_REWARD_PARAMS['SPEED']['IDEAL']:.1f}m/s)\n")
                f.write(f"  速度范围要求: {SPEED_LOWER_LIMIT:.1f}m/s - {SPEED_UPPER_LIMIT:.1f}m/s\n")

                if speed < SPEED_LOWER_LIMIT:
                    speed_note = "速度过低"
                elif speed <= TOUCHDOWN_REWARD_PARAMS['SPEED']['IDEAL']:
                    speed_note = "理想速度"
                elif speed <= TOUCHDOWN_REWARD_PARAMS['SPEED']['IDEAL'] + TOUCHDOWN_REWARD_PARAMS['SPEED']['TOLERANCE']:
                    speed_note = "可接受速度"
                elif speed <= SPEED_UPPER_LIMIT:
                    speed_note = "速度较高但在范围内"
                else:
                    speed_note = "速度过高，超出安全范围"

                f.write(f"    速度评估: {speed_note}\n")

                f.write(
                    f"  垂直速度: {vertical_speed:.1f}m/s (目标: {TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['IDEAL']:.1f}m/s)\n")

                if vertical_speed >= TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['IDEAL']:
                    vs_note = "理想下降率"
                elif vertical_speed >= TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['MAX']:
                    vs_note = "可接受下降率"
                else:
                    vs_note = "下降率过大"

                f.write(f"    下降率评估: {vs_note}\n")

                f.write(f"  姿态:\n")
                f.write(
                    f"    俯仰角: {pitch:.1f}° (目标: {TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['IDEAL']:.1f}°)\n")
                f.write(
                    f"    滚转角: {bank:.1f}° (目标: 0.0°, 最大允许: {TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MAX']:.1f}°)\n")

                pitch_error = abs(pitch - TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['IDEAL'])
                if pitch_error <= TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['TOLERANCE']:
                    pitch_note = "理想俯仰角"
                elif pitch < 0:
                    pitch_note = "危险俯仰角（机头下压）"
                else:
                    pitch_note = "非理想俯仰角"

                f.write(f"    俯仰评估: {pitch_note}\n")

                if abs(bank) <= TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MAX'] / 2:
                    bank_note = "理想滚转角"
                elif abs(bank) <= TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MAX']:
                    bank_note = "可接受滚转角"
                else:
                    bank_note = "危险滚转角"

                f.write(f"    滚转评估: {bank_note}\n")

                f.write("\n奖励分析:\n")
                f.write(f"  位置奖励: {rewards['position_reward']:.1f}\n")
                f.write(f"  速度奖励: {rewards['speed_reward']:.1f}\n")
                f.write(f"  垂直速度奖励: {rewards['vs_reward']:.1f}\n")
                f.write(f"  俯仰奖励: {rewards['pitch_reward']:.1f}\n")
                f.write(f"  滚转奖励: {rewards['bank_reward']:.1f}\n")
                f.write(f"  总奖励: {rewards['total_reward']:.1f}\n\n")

                f.write("改进建议:\n")
                if rewards['position_reward'] < TOUCHDOWN_REWARD_PARAMS['POSITION']['MAX_REWARD'] * 0.7:
                    if runway_y < ideal_start:
                        f.write("  - 尝试在跑道上更靠后的位置接地，对准理想着陆区\n")
                    elif runway_y > ideal_end:
                        f.write("  - 尝试在跑道上更靠前的位置接地，对准理想着陆区\n")
                    if abs(runway_x) > RUNWAY_CONFIG['WIDTH'] / 4:
                        f.write("  - 改进跑道中心线对准，减少横向偏差\n")

                if rewards['speed_reward'] < TOUCHDOWN_REWARD_PARAMS['SPEED']['MAX_REWARD'] * 0.7:
                    if speed > TOUCHDOWN_REWARD_PARAMS['SPEED']['IDEAL']:
                        f.write("  - 着陆前降低速度，接近理想接地速度\n")

                if rewards['vs_reward'] < TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['MAX_REWARD'] * 0.7:
                    if vertical_speed < TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['MAX']:
                        f.write("  - 降低下降率，避免硬着陆\n")
                    elif vertical_speed > TOUCHDOWN_REWARD_PARAMS['VERTICAL_SPEED']['IDEAL']:
                        f.write("  - 保持更稳定的下降率，接近理想值\n")

                if rewards['pitch_reward'] < TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['MAX_REWARD'] * 0.7:
                    f.write(
                        f"  - 调整接地姿态，保持约{TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['PITCH']['IDEAL']}度的俯仰角\n")

                if rewards['bank_reward'] < TOUCHDOWN_REWARD_PARAMS['ATTITUDE']['BANK']['MAX_REWARD'] * 0.7:
                    f.write("  - 保持机翼水平，减少接地时的滚转角\n")

            logger.info(f"着陆评估报告已保存至 {report_file}")

        except Exception as e:
            logger.error(f"生成评估报告时出错: {str(e)}")
            logger.error(traceback.format_exc())

    def _plot_landing_reward_progression(self):
        """
        创建显示着陆过程中奖励变化的图表
        记录从进入下滑道到着陆完成的整个过程中奖励的变化趋势
        """
        os.makedirs(RESULTS_DIR, exist_ok=True)

        # 读取CSV文件获取奖励数据
        rewards_data = []
        try:
            with open(self.data_file, 'r') as csvfile:
                reader = csv.DictReader(csvfile)
                for row in reader:
                    if 'total_reward' in row:
                        rewards_data.append(float(row['total_reward']))
        except Exception as e:
            logger.error(f"读取奖励数据失败: {str(e)}")
            return

        if not rewards_data:
            logger.info("没有奖励数据，无法生成图表")
            return

        try:
            # 创建图表
            plt.figure(figsize=(12, 8))

            # 设置背景色
            plt.gca().set_facecolor('#f0f0f8')
            plt.gcf().set_facecolor('#f0f0f8')

            # 绘制奖励曲线
            plt.plot(range(len(rewards_data)), rewards_data, 'b-', linewidth=1.5)

            # 设置标签和标题
            plt.xlabel('Time Step')  # 或 'Data Point' 或 'Landing Progression'
            plt.ylabel('Reward')
            plt.title('Reward Progression During Landing')  # 更新标题

            # 添加网格
            plt.grid(True, alpha=0.3)

            # 保存图表
            plt.tight_layout()
            plt.savefig(os.path.join(RESULTS_DIR, 'episode_reward_over_time.png'))
            plt.close()

            logger.info("奖励曲线图已保存至 'episode_reward_over_time.png'")

        except Exception as e:
            logger.error(f"生成奖励图表时出错: {str(e)}")
            logger.error(traceback.format_exc())


def main():
    """主函数：运行跑道接地奖励系统"""
    logger.info("启动跑道接地奖励系统")

    try:
        # 创建接地奖励系统
        touchdown_system = TouchdownRewardSystem()

        # 运行系统
        touchdown_system.run(max_time=600.0)  # 最多运行10分钟


    except KeyboardInterrupt:

        logger.info("用户中断系统")

        # 确保在中断时也生成奖励分析图

        if touchdown_system.touchdown_detected:
            touchdown_system._plot_reward_analysis()

    except Exception as e:

        logger.error(f"系统错误: {str(e)}")

        logger.error(traceback.format_exc())

    finally:

        # 确保在程序结束时生成奖励分析图

        # 替换为：
        if hasattr(touchdown_system, 'touchdown_detected') and touchdown_system.touchdown_detected:
            touchdown_system._plot_episode_reward_over_time()

        logger.info("跑道接地奖励系统已关闭")


if __name__ == "__main__":
    main()